//
// Created by LiuDongPeng on 2023/11/16.
//

#include "pid.h"


int pid_ctrl_init(pid_ctrl_t *pid, float p, float i, float d, float lowLimit, float upLimit)
{
    if (pid == 0)
        return -1;

    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->lowLimit = lowLimit;
    pid->upLimit = upLimit;

    pid->errorSumMax = 1.0f;

    return 0;
}


/**
 * @brief Get pi controller calc result
 * @param pid
 * @param tarIdq
 * @param fdkIdq
 * @param udq
 * @return
 */
float pid_ctrl_current_calc(pid_ctrl_t* pid, float tarIdq, float fdkIdq, float* udq)
{
    if (pid == 0 || udq == 0)
        return 0;

    pid->target = tarIdq;
    pid->value = fdkIdq;

    pid->error = pid->target - pid->value; // 误差 = 目标值 - 测量值
    pid->errorSum += pid->error;

    /* 积分限幅 */
    if (pid->errorSum > pid->errorSumMax)
    {
        pid->errorSum = pid->errorSumMax;
    }
    else if (pid->errorSum < -pid->errorSumMax)
    {
        pid->errorSum = -pid->errorSumMax;
    }
    else
    {
    }

    /* 得到PI运算结果 */
    pid->result = (pid->kp * pid->error) + (pid->ki * pid->errorSum)/* + (pid->kd * pid->error - pid->lastError)*/;

    /* 最大输出限幅 */
    if (pid->result > pid->upLimit)
    {
        pid->result = pid->upLimit;
    }
    else if (pid->result < pid->lowLimit)
    {
        pid->result = pid->lowLimit;
    }
    else
    {
    }
	*udq = pid->result;

	pid->lastError = pid->error;

    return pid->result;
}


